var mode = 'stop';

function init(address, port) {
	var ip = "ws://" + address + ":" + port;
	
	if(connect(ip)){
	}
}

function run() {
	subscribe('/sensorPacket', sensorPacketHandler);
	subscribe('/odom', odomHandler);
}

function stop() {
	if(mode == 'demo'){
		demo(255);
	}else if(mode == 'manual'){
		move_robot(0, 0);
	}
	mode = 'stop';
}

function up() {
	stop();
	return move_robot(0.1, 0);
}

function down() {
	stop();
	return move_robot(-0.1, 0);
}

function left() {
	stop();
	return move_robot(0.1, 1);
}

function right() {
	stop();
	return move_robot(0.1, -1);
}

function demo(x){
	stop();
	ros.callService('/demo', '[' + x + ']'); 
	mode == 'demo';
}

function move_robot(x, z) {
	mode = 'manual';
	return publish('/cmd_vel', 'geometry_msgs/Twist', '{"linear":{"x":' + x + ',"y":0,"z":0}, "angular":{"x":0,"y":0,"z":' + z + '}}');
}